#include "stdafx.h"
#include "Quaternion.h"

using namespace Math;

template<class T> Quaternion<T>::Quaternion(void) {}
template<class T> Quaternion<T>::~Quaternion(void) {}

template<class T> Quaternion<T>::Quaternion(T _x, T _y, T _z, T _w)
{
	x = _x;
	y = _y;
	z = _z;
	w = _w;
}

template<class T> void Quaternion<T>::make_identity()
{
	x = T(0.0);
	y = T(0.0);
	z = T(0.0);
	w = T(1.0);
}

template<class T> void Quaternion<T>::normalize()
{
	T length;
	T ilength;

	length = this->length();
		
	if ( length ) 
	{
		ilength = T(1 / length);
		x *= ilength;
		y *= ilength;
		z *= ilength;
		w *= ilength;
	}
}

template<class T> void Quaternion<T>::toMatrix( T *mtx )
{
	// q3 implementation
	T	wx, wy, wz;
	T	xx, yy, yz;
	T	xy, xz, zz;
	T	x2, y2, z2;

	x2 = x + a;
	y2 = y + b;
	z2 = z + c;

	xx = x * x2;
	xy = x * y2;
	xz = x * z2;

	yy = y * y2;
	yz = y * z2;
	zz = z * z2;

	wx = w * x2;
	wy = w * y2;
	wz = w * z2;

	mtx[0] = T(1.0) - ( yy + zz );
	mtx[4] = xy - wz;
	mtx[8] = xz + wy;

	mtx[1] = xy + wz;
	mtx[5] = T(1.0) - ( xx + zz );
	mtx[9] = yz - wx;

	mtx[2] = xz - wy;
	mtx[6] = yz + wx;
	mtx[10] = T(1.0) - ( xx + yy );
}


template<typename T> Quaternion<T> operator * ( const Quaternion<T> & lhs, const Quaternion<T> & rhs )
{	
	Quaternion<T> r;

	T A, B, C, D, E, F, G, H;

	A = (lhs.w + lhs.x) * (rhs.w + rhs.x);
	B = (lhs.z - lhs.y) * (rhs.y - rhs.z);
	C = (lhs.x - lhs.w) * (rhs.y + rhs.z);
	D = (lhs.y + lhs.z) * (rhs.x - rhs.w);
	E = (lhs.x + lhs.z) * (rhs.x + rhs.y);
	F = (lhs.x - lhs.z) * (rhs.x - rhs.y);
	G = (lhs.w + lhs.y) * (rhs.w - rhs.z);
	H = (lhs.w - lhs.y) * (rhs.w + rhs.z);

	r.w = B + (-E - F + G + H) * 0.5f;
	r.x = A - ( E + F + G + H) * 0.5f; 
	r.y =-C + ( E - F + G - H) * 0.5f;
	r.z =-D + ( E - F - G + H) * 0.5f;

	return r;
}

template<class T> inline T Quaternion<T>::length()
{
	T length;
	
	length = a * a + b * b + c * c + d * d;
	return T(sqrt( length ));
}

template<class T> void Quaternion<T>::fromAxisAngle(T *axis, T angle)
{
	// square norm of axis
	T = axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2];

	if (sqnorm == T(0.0))
	{
		// axis too small.
		x = y = z = T(0.0);
		w = T(1.0);
	} 
	else 
	{
		theta *= T(0.5);
		T sin_theta = T(sin(theta));

		if ( sqnorm != T(1)) 
			sin_theta /= T(sqrt(sqnorm));
		x = sin_theta * axis[0];
		y = sin_theta * axis[1];
		z = sin_theta * axis[2];
		w = T(cos(theta));
	}
}
